import zmq
import sys
import time
import smbus
from random import sample
from rpi_ws281x import Adafruit_NeoPixel, Color
sys.path.append('..')
from data_gathering.MPU6050 import mpu6050_action


# LED parameter settings
LED_COLOR = (255, 0, 0)     # color of active LED
LED_COUNT = 32              # Number of LED pixels.
LED_PIN = 18                # GPIO pin connected to the pixels (must support PWM!).
LED_FREQ_HZ = 800000        # LED signal frequency in hertz (usually 800khz)
LED_DMA = 10                # DMA channel to use for generating signal (try 5)
LED_BRIGHTNESS = 10         # Set to 0 for darkest and 255 for brightest
LED_INVERT = False          # True to invert the signal (when using NPN transistor level shift)


def light_on(strip):
    for i in range(strip.numPixels()):
        strip.setPixelColor(i, Color(*LED_COLOR))
    strip.show()


def light_off(strip):
    for i in range(strip.numPixels()):
        strip.setPixelColor(i, Color(0, 0, 0))
    strip.show()
# END of LED parameter settings


# MPU6050 settings
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68 # This is the address value read via the i2cdetect command
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
# END of MPU6050 settings


broker_ip = sys.argv[1]
agent_id = int(sys.argv[2])
# broker_ip = '192.168.137.79'
# agent_id = 2

up, down, left, right = False, False, False, False


def upload_data(publisher, x_rotation, y_rotation):
    next_agent = 1  # center = 1
    publisher.send_string('%d:%f:%f' % (next_agent, x_rotation, y_rotation))
    print("x rotation: " + str(x_rotation))
    print("y rotation: " + str(y_rotation))


def decode(x_rotation, y_rotation):
    # x_distance, y_distance, rotation = -50, 50, 1
    global up
    global down
    global left
    global right
    # Calculate by x_rotation, y_rotation, can be extended to velocity.
    up, down, left, right = True, False, False, False


def main(agent_id):
    context = zmq.Context()
    publisher = context.socket(zmq.PUB)
    publisher.connect("tcp://%s:5556" % broker_ip)

    subscriber = context.socket(zmq.SUB)
    subscriber.connect("tcp://%s:5555" % broker_ip)
    subscriber.setsockopt_string(zmq.SUBSCRIBE, str(agent_id))
    strip = Adafruit_NeoPixel(LED_COUNT, LED_PIN, LED_FREQ_HZ, LED_DMA, LED_INVERT, LED_BRIGHTNESS)
    strip.begin()
    while True:
        # recv data of hand moving, output to screen
        if agent_id == 1:
            message = subscriber.recv()
            # data format: float x:float y, for example: -5.5195:-9.3865
            rotation = str(message, encoding="utf8")[2:]
            x_rotation, y_rotation = rotation.split(':')
            print('%d:%s:%s' % (agent_id, x_rotation, y_rotation))
            decode(x_rotation, y_rotation)  # direction vector is global.

        # hand move, send data to agent1
        elif agent_id == 2 or agent_id == 3:
            time.sleep(0.1)
            x_rotation, y_rotation = mpu6050_action(bus, address)
            upload_data(publisher, x_rotation, y_rotation)
            time.sleep(0.5)

        # wrong agent ID!
        else:
            light_on(strip)
            time.sleep(3.0)
            print("wrong agent ID!")
            light_off(strip)


if __name__ == "__main__":
    main(agent_id)
